#include "vtkVelodyneAdvancedPacketInterpreter.h"
#include "VelodyneAPFCommon.h"

#include <vtkDoubleArray.h>
#include <vtkObjectFactory.h>
#include <vtkPointData.h>
#include <vtkPoints.h>
#include <vtkTransform.h>

#include <type_traits>

#include <cstring>
#include <bitset>
#include <iostream>

// include only to go the structure VelodyneSpecificFrameInformation before merging code
#include <vtkVelodyneLegacyPacketInterpreter.h>
#include "VelodyneInterpreterCommon.h"

//------------------------------------------------------------------------------
// vtkVelodyneAdvancedPacketInterpreter methods.
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
/*!
 * @brief         Initialize an array for datapoint attributes and add it to the
 *                polyData.
 * @tparam        T                The type of the array. This is templated so
 *                                 that the caller does not need to consider the
 *                                 type, which may change with the
 *                                 specification.
 * @param[in,out] array            The input array.
 * @param[in]     numberOfElements The number of elements that the array must be
 *                                 able to hold after initialization.
 * @param[out]    polyData         The PolyData instance to which the array
 *                                 should be added.
 */
template <typename T>
inline void
InitializeDataArrayForPolyData(
  T & array,
  char const * name,
  vtkIdType numberOfElements,
  vtkPolyData * polyData)
{
  array = T::New();
  array->SetNumberOfValues(numberOfElements);
  array->SetName(name);
  polyData->GetPointData()->AddArray(array);
}

//------------------------------------------------------------------------------
vtkStandardNewMacro(vtkVelodyneAdvancedPacketInterpreter)

//----------------------------------------------------------------------------
vtkVelodyneAdvancedPacketInterpreter::vtkVelodyneAdvancedPacketInterpreter()
{
  this->CurrentFrameTracker          = new FrameTracker();
  this->MaxFrameSize                 = MEM_STEP_SIZE;
  this->CurrentArraySize             = 0;
  this->NumberOfPointsInCurrentFrame = 0;
  this->ResetCurrentFrame();

  this->ParserMetaData.SpecificInformation =
    std::make_shared<VelodyneSpecificFrameInformation>();

  this->LaserSelection->SetNumberOfTuples(HDL_MAX_NUM_LASERS);
  this->LaserSelection->Fill(static_cast<int>(true));
}

//------------------------------------------------------------------------------
vtkVelodyneAdvancedPacketInterpreter::~vtkVelodyneAdvancedPacketInterpreter()
{
  delete this->CurrentFrameTracker;
}

//------------------------------------------------------------------------------
void
vtkVelodyneAdvancedPacketInterpreter::ProcessPacket(
  const unsigned char * data,
  unsigned int dataLength)
{
  decltype(dataLength) index = 0;
  PayloadHeader const * payloadHeader =
    reinterpretCastWithChecks<PayloadHeader>(data, dataLength, index);
  if (payloadHeader == nullptr)
  {
    return;
  }
  ADVANCE_INDEX_BY_HLEN_OR_RETURN(dataLength, index, payloadHeader, void())

  // trim the trailer from the effective length
  decltype(dataLength) payloadLength = dataLength - payloadHeader->GetTlen();

  uint8_t const distanceCount = payloadHeader->GetDistanceCount();

  // A valid distance count may be 0, in which case there are no returns
  // included in the firing and thus there is nothing to do.
  if (distanceCount == 0)
  {
    return;
  }

  uint8_t const distanceSize = payloadHeader->GetDistanceSizeInBytes();
  uint8_t distanceIndex;

  auto const pseq       = payloadHeader->GetPseq();
  auto const iset       = payloadHeader->GetIset();
  auto const dsetMask   = payloadHeader->GetDsetMask();
  auto const isDsetMask = payloadHeader->IsDsetMask();

  // 64-bit PTP truncated format.
  // auto const timeRef = payloadHeader->GetTref();
  auto const packetTimeReferenceInNanoseconds = payloadHeader->GetTrefInNanoseconds();

  size_t const numberOfBytesPerFiringGroupHeader = payloadHeader->GetGlen();
  size_t const numberOfBytesPerFiringHeader      = payloadHeader->GetFlen();
  size_t const numberOfBytesPerFiringReturn = payloadHeader->GetNumberOfBytesPerFiringReturn();
  size_t const numberOfBytesPerFiring = payloadHeader->GetNumberOfBytesPerFiring();

  // Skip optional extension headers.
  auto nxhdr = payloadHeader->GetNxhdr();
  while (nxhdr != 0)
  {
    ExtensionHeader const * extensionHeader =
      reinterpretCastWithChecks<ExtensionHeader>(data, payloadLength, index);
    if (extensionHeader == nullptr)
    {
      return;
    }
    ADVANCE_INDEX_BY_HLEN_OR_RETURN(payloadLength, index, extensionHeader, void())
    nxhdr = extensionHeader->GetNxhdr();
  }

  // The included distance types may be specified by a bit mask in DSET, for
  // example 0110 indicates the presence of distance type 0100 and 0010. To
  // display this information, we need to retrieve the type in the firing loop
  // below. To avoid redundant bit calculations to retrieve the nth bit from the
  // mask, we cache the results in a vector instead. The order of the distance
  // types is determined by their bit values in the mask per the specification
  // so this is safe.

  // To do this, we start with the DSET mask and determine the value of the
  // first set bit. This is the distance type of the first distance. We then
  // remove that bit from the mask by subtraction and proceed to the next bit
  // and repeat. The values are stored successively so that they can be indexed
  // below.
  std::remove_const<decltype(dsetMask)>::type dsetRemainingMask = dsetMask;
  decltype(dsetRemainingMask) dsetBit;
  std::vector<decltype(dsetRemainingMask)> distanceTypes;
  for (distanceIndex = 0; distanceIndex < distanceCount; ++distanceIndex)
  {
    if (isDsetMask)
    {
      dsetBit = FIRST_SET_BIT(dsetRemainingMask);
      distanceTypes.push_back(dsetBit);
      dsetRemainingMask -= dsetBit;
    }
    // DSET may specify a count instead of a mask, in which case the distance
    // types are not specified. Use 0 in that case, which has no meaning in this
    // context.
    else
    {
      distanceTypes.push_back(0);
    }
  }

  // Update the transforms here and then call internal transform
  if (SensorTransform) this->SensorTransform->Update();

  // Resize the arrays if necessary.
  size_t currentArraySize = this->Points->GetNumberOfPoints();
  size_t safeArraySize    = this->NumberOfPointsInCurrentFrame +
                         payloadHeader->MaximumNumberOfPointsPerPacket();
  if (currentArraySize < safeArraySize)
  {
    this->SetNumberOfItems(safeArraySize);
  }

  VelodyneSpecificFrameInformation* velodyneFrameInfo =
    reinterpret_cast<VelodyneSpecificFrameInformation*>(
    this->ParserMetaData.SpecificInformation.get());
  size_t firingBlock = static_cast<size_t>(velodyneFrameInfo->FiringToSkip);
  velodyneFrameInfo->FiringToSkip = 0;

  // Loop through firing groups until a frame shift is detected. The number of
  // firings in each group is variable so we need to step through all of them to
  // get to the startPosition calculated by PreProcessPacket.
  size_t loopCount = 0;
  while (index < payloadLength)
  {
    align_to_word_size(index);
    FiringGroupHeader const * firingGroupHeader =
      reinterpretCastWithChecks<FiringGroupHeader>(data, payloadLength, index);
    if (firingGroupHeader == nullptr)
    {
      return;
    }
    // The payload header checks above ensure that this value is non-zero and
    // that the loop will therefore eventually terminate.
    index += numberOfBytesPerFiringGroupHeader;

    // Skip the firings and jump to the next firing group header.
    if ((loopCount++) < firingBlock)
    {
      index += numberOfBytesPerFiring * firingGroupHeader->GetFcnt();
      continue;
    }

    bool isNewFrame = this->CurrentFrameTracker->Update(firingGroupHeader->GetAzm(),
                                                        firingGroupHeader->GetVdfl(),
                                                        firingGroupHeader->GetHdir(),
                                                        firingGroupHeader->GetVdir());
    if (isNewFrame)
    {
      this->SplitFrame();
    }

    auto const timeFractionOffsetInNanoseconds         = firingGroupHeader->GetToffs();
    auto const coChannelSpan                           = firingGroupHeader->GetFspn();
    auto const coChannelTimeFractionDelayInNanoseconds = firingGroupHeader->GetFdly();
    auto const vdfl                                    = firingGroupHeader->GetVdfl();
    // double const verticalAngleInDegrees             = firingGroupHeader->GetVerticalDeflection();
    auto const azimuth                                 = firingGroupHeader->GetAzm();
    double const azimuthInDegrees                      = firingGroupHeader->GetAzimuth();
    auto const numberOfFirings                         = firingGroupHeader->GetFcnt();

    for (
      std::remove_const<decltype(numberOfFirings)>::type i = 0;
      i < numberOfFirings;
      ++i
    )
    {
      // TODO
      // This assumes that the spans are returned in order in the firing group.
      // Check that this is the case. If not, determine how to handle this
      // (e.g. by using the channel number?).

      // Intentional truncating integer division. The span index counts the
      // number of spans and should only increment after multiples of
      // coChannelSpan.
      // Also note that these operations are safe despite the mixed types due
      // to the rules for implicit integer promotion.
      decltype(timeFractionOffsetInNanoseconds) iSpan = i / coChannelSpan;
      decltype(timeFractionOffsetInNanoseconds) channelTimeFractionOffsetInNanoseconds =
        timeFractionOffsetInNanoseconds + (coChannelTimeFractionDelayInNanoseconds * iSpan);

      decltype(packetTimeReferenceInNanoseconds) firingTimeInNanoseconds =
        packetTimeReferenceInNanoseconds + channelTimeFractionOffsetInNanoseconds;

      FiringHeader const * firingHeader = reinterpretCastWithChecks<FiringHeader>(data, payloadLength, index);
      if (firingHeader == nullptr)
      {
        return;
      }
      index += numberOfBytesPerFiringHeader;

      auto const channelNumber = firingHeader->GetLcn();
      // only process point when the laser is selected
      if (!static_cast<bool>(this->LaserSelection->GetTuple1(static_cast<unsigned int>(channelNumber))))
      {
        index += numberOfBytesPerFiringReturn * distanceCount;
        continue;
      }

      // auto const firingMode = firingHeader->GetFm();
      // auto const firingModeString = toString(firingMode);
      auto const power = firingHeader->GetPwr();
      auto const noise = firingHeader->GetNf();
      // Status is also an enum and requires a string conversion.
      // auto const status = firingHeader->GetStat();
      // auto const statusString = toString(status);

      for (distanceIndex = 0; distanceIndex < distanceCount; ++distanceIndex)
      {
        // Given the checks in place in IsLidarPacket and above, this should not
        // be necessary because this point of code should only be reached by
        // well-formed packets of the expected length.
        //
        // This was added due to a bug reported about a crash when no intensity
        // values are provided. The test data was not provided so the cause
        // could not be determined with certainty. This check was added as a
        // precaution but it may be unnecessary and unrelated.
        decltype(dataLength) availableBytes = ((payloadLength > index) ? payloadLength - index : 0);
        if (availableBytes > numberOfBytesPerFiringReturn)
        {
          availableBytes = numberOfBytesPerFiringReturn;
        }
        FiringReturn firingReturn(data + index, availableBytes);
        index += numberOfBytesPerFiringReturn;

        uint32_t const distance = firingReturn.GetDistance<uint32_t>(distanceSize);
        if (this->IgnoreZeroDistances && distance == 0)
        {
          continue;
        }

        RawValues rawValues(azimuth, vdfl, distance);
        CorrectedValues correctedValues;
        double (& position)[3] = correctedValues.position;

        this->ComputeCorrectedValues(
          rawValues,
          channelNumber,
          correctedValues,
          false);

        // Apply sensor transform
        if (SensorTransform) this->SensorTransform->InternalTransformPoint(position, position);

        // Check if the point should be cropped out.
        if (this->shouldBeCroppedOut(position))
        {
          continue;
        }

        auto arrayIndex = this->NumberOfPointsInCurrentFrame++;

//! @brief Convencience macro for setting info array values.
#define VAPI_SET_VALUE(my_array, value)                                        \
  if (this->INFO_##my_array != nullptr)                                        \
  {                                                                            \
    this->INFO_##my_array->SetValue(arrayIndex, value);                        \
  }

        // We start with the confidence information to avoid adding the point
        // if it has to be skipped because of the DROP value
        if (this->INFO_Confidences != nullptr)
        {
          // The "INFO_Confidences" array is always created,
          // even if there is no confidence information in the packet.
          // If there is confidence information in the packet :
          // 5 arrays are created that correspond to the "details" confidence information.
          if(iset & (ISET_CONFIDENCE))
          {
            // The confidence value is present in the packet
            uint32_t confidenceValue = firingReturn.GetIntensity<uint32_t>( distanceSize, iset, (ISET_CONFIDENCE));

            const ApfConfidence* confidence_struct = reinterpret_cast<const ApfConfidence*>(&confidenceValue);
            if(confidence_struct)
            {
              if(!this->INFO_SNR)
              {
                // We create the details confidence arrays here
                // So the user does not see 5 arrays fill of 0
                // In case there is no confidence information in the packet
                InitializeDataArrayForPolyData(this->INFO_SNR, "SNR", this->MaxFrameSize, this->CurrentFrame);
                InitializeDataArrayForPolyData(this->INFO_INTF, "INTF", this->MaxFrameSize, this->CurrentFrame);
                InitializeDataArrayForPolyData(this->INFO_REHI, "REHI", this->MaxFrameSize, this->CurrentFrame);
                InitializeDataArrayForPolyData(this->INFO_RESH, "RESH", this->MaxFrameSize, this->CurrentFrame);
                InitializeDataArrayForPolyData(this->INFO_DROP, "DROP", this->MaxFrameSize, this->CurrentFrame);

                // We have to fill every arrays with 0
                // In case the confidence value is not available for the first points of the packet
                // Otherwise the value of the first points will be wrong
                this->INFO_SNR->Fill(0);
                this->INFO_INTF->Fill(0);
                this->INFO_REHI->Fill(0);
                this->INFO_RESH->Fill(0);
                this->INFO_DROP->Fill(0);
              }

              // Skip the point if the drop value says so
              if(this->HideDropPoints && confidence_struct->GetDROP())
              {
                this->NumberOfPointsInCurrentFrame--;
                continue;
              }

              vtkStdString confidenceString = std::bitset<8>(confidenceValue).to_string();
              this->INFO_Confidences->SetValue(arrayIndex, confidenceString);

              VAPI_SET_VALUE(SNR, confidence_struct->GetSNR())
              VAPI_SET_VALUE(INTF, confidence_struct->GetINTF())
              VAPI_SET_VALUE(REHI, confidence_struct->GetREHI())
              VAPI_SET_VALUE(RESH, confidence_struct->GetRESH())
              VAPI_SET_VALUE(DROP, confidence_struct->GetDROP())
            }
          }
          else
          {
            // If the confidence value is not present in the packet, we set the array to 0
            vtkStdString confidenceString = std::bitset<8>(0).to_string();
            this->INFO_Confidences->SetValue(arrayIndex, confidenceString);
          }
        }

        this->Points->SetPoint(arrayIndex, position);

        auto distType = distanceTypes[distanceIndex];
        VAPI_SET_VALUE(Xs, position[0])
        VAPI_SET_VALUE(Ys, position[1])
        VAPI_SET_VALUE(Zs, position[2])
        VAPI_SET_VALUE(Azimuths, azimuthInDegrees)
        VAPI_SET_VALUE(Distances, correctedValues.distance)
        VAPI_SET_VALUE(RawDistances, distance)
        VAPI_SET_VALUE(DistanceTypes, distType)
        // VAPI_SET_VALUE(DistanceTypeStrings, toString(toDistanceType(distType)))
        VAPI_SET_VALUE(Pseqs, pseq)
        VAPI_SET_VALUE(ChannelNumbers, channelNumber)
        VAPI_SET_VALUE(TimeFractionOffsets, channelTimeFractionOffsetInNanoseconds)
        VAPI_SET_VALUE(Timestamps, firingTimeInNanoseconds)
        VAPI_SET_VALUE(Powers, power)
        VAPI_SET_VALUE(Noises, noise)
        VAPI_SET_VALUE(VerticalAngles, correctedValues.elevation)

        //! @brief Convenience macro for setting intensity values
#define VAPI_INSERT_INTENSITY(my_array, iset_flag)                             \
  if (this->INFO_##my_array != nullptr)                                        \
  {                                                                            \
    this->INFO_##my_array->SetValue(                                           \
      arrayIndex,                                                              \
      (iset & (ISET_##iset_flag)) ? firingReturn.GetIntensity<uint32_t>(       \
                                      distanceSize, iset, (ISET_##iset_flag))  \
                                  : 0);                                        \
  }

        // TODO: Make the inclusion of these columns fully optional at runtime.

        // Add additional values here when ISET is expanded in future versions.
        VAPI_INSERT_INTENSITY(Reflectivities, REFLECTIVITY)
        VAPI_INSERT_INTENSITY(Intensities, INTENSITY)
      }
    }
  }
}

//------------------------------------------------------------------------------
bool
vtkVelodyneAdvancedPacketInterpreter::IsLidarPacket(
  unsigned char const * data,
  unsigned int dataLength)
{
  decltype(dataLength) index = 0;

  // This checks that PayloadHeader's IsValid function, which in turn checks
  // that the version is 1 and that expected lengths are consistent.
  PayloadHeader const * payloadHeader =
    reinterpretCastWithChecks<PayloadHeader>(data, dataLength, index);
  if ((payloadHeader == nullptr) || (payloadHeader->GetHlen() > dataLength))
  {
    return false;
  }
  ADVANCE_INDEX_BY_HLEN_OR_RETURN(dataLength, index, payloadHeader, false);

  // trim the trailer from the effective length
  decltype(dataLength) payloadLength = dataLength - payloadHeader->GetTlen();

  auto nxhdr = payloadHeader->GetNxhdr();
  while (nxhdr != 0)
  {
    ExtensionHeader const * extensionHeader =
      reinterpretCastWithChecks<ExtensionHeader>(data, payloadLength, index);
    if (extensionHeader == nullptr)
    {
      return false;
    }
    ADVANCE_INDEX_BY_HLEN_OR_RETURN(payloadLength, index, extensionHeader, false);
    nxhdr = extensionHeader->GetNxhdr();
  }

  // Check for empty distance counts, which mean there are no firings.
  if (payloadHeader->GetDistanceCount() != 0)
  {
    size_t numberOfBytesPerFiring = payloadHeader->GetNumberOfBytesPerFiring();
    size_t numberOfBytesPerFiringGroupHeader = payloadHeader->GetGlen();

    while (index < payloadLength)
    {
      align_to_word_size(index);
      FiringGroupHeader const * firingGroupHeader =
        reinterpretCastWithChecks<FiringGroupHeader>(data, payloadLength, index);
      if (firingGroupHeader == nullptr)
      {
        return false;
      }
      // TODO
      // Add firing header checks if necessary here. See ProcessPacket for an
      // example of how to loop over each firing and advance the index.
      index += (numberOfBytesPerFiring * firingGroupHeader->GetFcnt()) +
        numberOfBytesPerFiringGroupHeader;
    }
  }

  // return true;
  return index == payloadLength;
}

//------------------------------------------------------------------------------
vtkSmartPointer<vtkPolyData>
vtkVelodyneAdvancedPacketInterpreter::CreateNewEmptyFrame(
  vtkIdType numberOfPoints,
  vtkIdType vtkNotUsed(prereservedNumberOfPoints))
{
  vtkSmartPointer<vtkPolyData> polyData = vtkSmartPointer<vtkPolyData>::New();

  this->UpdateMaxFrameSize(numberOfPoints);

  // Points.
  vtkNew<vtkPoints> points;
  points->SetDataTypeToFloat();
  points->SetNumberOfPoints(this->MaxFrameSize);
  // Same name as vtkVelodyneHDLReader.
  points->GetData()->SetName("Points_m_XYZ");

  // Replace the old points.
  this->Points = points.GetPointer();

  // Point the polyData to the points.
  polyData->SetPoints(points.GetPointer());

  // Replace and initialize all of the associated data arrays.

//! @brief Convencience macro for initializing info arrays.
#define VAPI_INIT_INFO_ARR(is_advanced, arr_name, disp_name)                   \
  if ((is_advanced && !this->EnableAdvancedArrays))                            \
  {                                                                            \
    this->INFO_##arr_name = nullptr;                                           \
  }                                                                            \
  else                                                                         \
  {                                                                            \
    InitializeDataArrayForPolyData(                                            \
      this->INFO_##arr_name, disp_name, this->MaxFrameSize, polyData);         \
  }

  VAPI_INIT_INFO_ARR(true, Xs, "X")
  VAPI_INIT_INFO_ARR(true, Ys, "Y")
  VAPI_INIT_INFO_ARR(true, Zs, "Z")
  VAPI_INIT_INFO_ARR(false, Distances, "distance_m")
  VAPI_INIT_INFO_ARR(true, RawDistances, "distance_raw")
  VAPI_INIT_INFO_ARR(true, DistanceTypes, "distance_type")
  VAPI_INIT_INFO_ARR(false, Azimuths, "azimuth")
  VAPI_INIT_INFO_ARR(false, VerticalAngles, "vertical_angle")
/*
  VAPI_INIT_INFO_ARR(true, DistanceTypeStrings  , "distance_type_name")
  VAPI_INIT_INFO_ARR(true, FiringModeStrings    , "firing_mode")
  VAPI_INIT_INFO_ARR(true, StatusStrings        , "status")
*/
  VAPI_INIT_INFO_ARR(false, Intensities, "intensity")
  VAPI_INIT_INFO_ARR(true, Confidences, "confidence")
  VAPI_INIT_INFO_ARR(false, Reflectivities, "reflectivity")
  VAPI_INIT_INFO_ARR(true, ChannelNumbers, "LCN")
  VAPI_INIT_INFO_ARR(true, TimeFractionOffsets, "time_fraction_offset")
  VAPI_INIT_INFO_ARR(false, Timestamps, "timestamp")
  VAPI_INIT_INFO_ARR(true, Powers, "power")
  VAPI_INIT_INFO_ARR(true, Noises, "noise")
  VAPI_INIT_INFO_ARR(true, Pseqs, "PSEQ")

  // Initialize this array to nullptr
  // They will be initialized in the process packet if confidence is detected in the packet
  this->INFO_SNR = nullptr;
  this->INFO_INTF = nullptr;
  this->INFO_REHI = nullptr;
  this->INFO_RESH = nullptr;
  this->INFO_DROP = nullptr;

  this->NumberOfPointsInCurrentFrame = 0;
  this->CurrentArraySize             = numberOfPoints;
  return polyData;
}


//------------------------------------------------------------------------------
// TODO: Revisit this if the frequency still needs to be calculated here.
bool
vtkVelodyneAdvancedPacketInterpreter::SplitFrame(bool force, FramingMethod_t framingMethodAskingForSplitFrame)
{
  auto numberOfAllocatedPoints = this->Points->GetNumberOfPoints();
  // Update the MaxId to the current number of points.
  this->SetNumberOfItems(this->NumberOfPointsInCurrentFrame);
  // this->CurrentFrame->Modified();
  bool wasSplit = this->vtkLidarPacketInterpreter::SplitFrame(force, framingMethodAskingForSplitFrame);
  // If the frame was split then CreateNewEmptyDataFrame was called and the
  // array sizes have already been adjusted. If not, we need to reset the MaxId
  // to allow for further insertions.
  if (!wasSplit)
  {
    this->SetNumberOfItems(numberOfAllocatedPoints);
  }
  return wasSplit;
}

//------------------------------------------------------------------------------
void
vtkVelodyneAdvancedPacketInterpreter::ResetCurrentFrame()
{
  this->CurrentFrame = this->CreateNewEmptyFrame(0);
  this->CurrentFrameTracker->Reset();
  this->Frames.clear();
}

//------------------------------------------------------------------------------
bool
vtkVelodyneAdvancedPacketInterpreter::PreProcessPacket(
  const unsigned char * data,
  unsigned int dataLength,
  fpos_t filePosition,
  double packetNetworkTime,
  std::vector<FrameInformation> * frameCatalog)
{
  this->ParserMetaData.FilePosition           = filePosition;
  this->ParserMetaData.FirstPacketNetworkTime = packetNetworkTime;
  //! @todo
  //  this->ParserMetaData.FirstPacketDataTime = packetNetworkTime;
  auto * velFrameInfo =
    reinterpret_cast<VelodyneSpecificFrameInformation *>(
      this->ParserMetaData.SpecificInformation.get());
  //  if (dataPacket->gpsTimestamp < this->lastGpsTimestamp)
  //  {
  //    velFrameInfo->NbrOfRollingTime++;
  //  }

  decltype(dataLength) index = 0;
  PayloadHeader const * payloadHeader =
    reinterpretCastWithChecks<PayloadHeader>(data, dataLength, index);
  if ((payloadHeader == nullptr) || (payloadHeader->GetDistanceCount() == 0))
  {
    return false;
  }
  ADVANCE_INDEX_BY_HLEN_OR_RETURN(dataLength, index, payloadHeader, false)

  // trim the trailer from the effective length
  decltype(dataLength) payloadLength = dataLength - payloadHeader->GetTlen();

  // Skip optional extension headers.
  auto nxhdr = payloadHeader->GetNxhdr();
  while (nxhdr != 0)
  {
    ExtensionHeader const * extensionHeader =
      reinterpretCastWithChecks<ExtensionHeader>(data, payloadLength, index);
    if (extensionHeader == nullptr)
    {
      return false;
    }
    ADVANCE_INDEX_BY_HLEN_OR_RETURN(payloadLength, index, extensionHeader, false)
    nxhdr = extensionHeader->GetNxhdr();
  }

  // Loop through firing groups until a frame shift is detected.
  size_t numberOfBytesPerFiringGroupHeader = payloadHeader->GetGlen();
  size_t numberOfBytesPerFiring = payloadHeader->GetNumberOfBytesPerFiring();
  int firingCount               = 0;
  bool isNewFrame               = false;
  while (index < payloadLength)
  {
    align_to_word_size(index);
    FiringGroupHeader const * firingGroupHeader =
      reinterpretCastWithChecks<FiringGroupHeader>(data, payloadLength, index);
    if (firingGroupHeader == nullptr)
    {
      return isNewFrame;
    }
    // The payload header checks above ensure that this value is non-zero and
    // that the loop will therefore eventually terminate.
    bool isNewFrame = this->CurrentFrameTracker->Update(firingGroupHeader->GetAzm(),
                                                        firingGroupHeader->GetVdfl(),
                                                        firingGroupHeader->GetHdir(),
                                                        firingGroupHeader->GetVdir());

    if (isNewFrame)
    {
      velFrameInfo->FiringToSkip = firingCount;
      if (frameCatalog)
      {
        frameCatalog->push_back(this->ParserMetaData);
      }
      // Create a copy of the current meta data state
      // at a different memory location than the one
      // added to the catalog
      return isNewFrame;
    }
    firingCount++;
    index += (numberOfBytesPerFiring * firingGroupHeader->GetFcnt()) +
             numberOfBytesPerFiringGroupHeader;
  }
  return isNewFrame;
}


//------------------------------------------------------------------------------
void
vtkVelodyneAdvancedPacketInterpreter::UpdateMaxFrameSize(size_t frameSize)
{
  if (frameSize > this->MaxFrameSize)
  {
    size_t difference = frameSize - this->MaxFrameSize;
    this->MaxFrameSize +=
      ((difference + (MEM_STEP_SIZE - 1)) / MEM_STEP_SIZE) * MEM_STEP_SIZE;
  }
}

//------------------------------------------------------------------------------
// Macro-based methods.
//------------------------------------------------------------------------------
void
vtkVelodyneAdvancedPacketInterpreter::ResizeArrays()
{
  size_t newSize     = this->MaxFrameSize;
  size_t currentSize = this->CurrentArraySize;
  if (newSize <= currentSize)
  {
    return;
  }

  this->Points->Resize(newSize);

#define VAPI_RESIZE(index, data, array)                                        \
  if (array != nullptr)                                                        \
  {                                                                            \
    array->Resize(newSize);                                                    \
  }

  // "data" is an unused placeholder
  VAPI_FOREACH_INFO_ARRAY(VAPI_RESIZE, data)

  this->CurrentArraySize = newSize;
}
//------------------------------------------------------------------------------
void
vtkVelodyneAdvancedPacketInterpreter::SetNumberOfItems(size_t numberOfItems)
{
  this->UpdateMaxFrameSize(numberOfItems);
  if (numberOfItems > static_cast<size_t>(this->Points->GetNumberOfPoints()))
  {
    this->ResizeArrays();
  }

  this->Points->SetNumberOfPoints(numberOfItems);

#define VAPI_SET_NUMBER_OF_VALUES(index, data, array)                          \
  if (array != nullptr)                                                        \
  {                                                                            \
    array->SetNumberOfValues(numberOfItems);                                   \
  }

  VAPI_FOREACH_INFO_ARRAY(VAPI_SET_NUMBER_OF_VALUES, data)

  if(this->INFO_SNR)
  {
    this->INFO_SNR->SetNumberOfValues(numberOfItems);
    this->INFO_INTF->SetNumberOfValues(numberOfItems);
    this->INFO_REHI->SetNumberOfValues(numberOfItems);
    this->INFO_RESH->SetNumberOfValues(numberOfItems);
    this->INFO_DROP->SetNumberOfValues(numberOfItems);
  }
}

//------------------------------------------------------------------------------
void vtkVelodyneAdvancedPacketInterpreter::LoadCalibration(const std::string& filename)
{
  this->vtkVelodyneBasePacketInterpreter::LoadCalibration(filename);
  this->CurrentFrameTracker->SetFramingLogic(this->GetFramingLogic());
}

//-----------------------------------------------------------------------------
std::string vtkVelodyneAdvancedPacketInterpreter::GetSensorName()
{
  return "APF-" + std::to_string(this->CalibrationReportedNumLasers);
}
